#pragma config(Hubs,  S1, HTMotor,  HTMotor,  HTMotor,  HTServo)
#pragma config(Sensor, S2,     HTPB,                sensorI2CCustom9V)
#pragma config(Sensor, S3,     SMUX_1,              sensorI2CCustom)
#pragma config(Sensor, S4,     SMUX_2,              sensorI2CCustom)
#pragma config(Motor,  mtr_S1_C1_1,     e,             tmotorNormal, openLoop)
#pragma config(Motor,  mtr_S1_C1_2,     a,             tmotorNormal, PIDControl, reversed, encoder)
#pragma config(Motor,  mtr_S1_C2_1,     b,             tmotorNormal, PIDControl, reversed, encoder)
#pragma config(Motor,  mtr_S1_C2_2,     c,             tmotorNormal, PIDControl, encoder)
#pragma config(Motor,  mtr_S1_C3_1,     f,             tmotorNormal, PIDControl, encoder)
#pragma config(Motor,  mtr_S1_C3_2,     d,             tmotorNormal, PIDControl, encoder)
#pragma config(Servo,  srvo_S1_C4_1,    frontarm,             tServoStandard)
#pragma config(Servo,  srvo_S1_C4_2,    backarm,              tServoStandard)
#pragma config(Servo,  srvo_S1_C4_3,    dispencer,            tServoStandard)
#pragma config(Servo,  srvo_S1_C4_4,    Irpullup,             tServoStandard)
#pragma config(Servo,  srvo_S1_C4_5,    Irfront,              tServoStandard)
#pragma config(Servo,  srvo_S1_C4_6,    Irback,               tServoStandard)
//*!!Code automatically generated by 'ROBOTC' configuration wizard               !!*//

/**********************************************************************\
*																																	    *
* PROGRAM: TeleOp program for Get Over It 2010-2011    	      			  *
* VERSION: 2.0  																									    *
* PURPOSE: Controllable user program for Tele-operational period.     *
Omni-bot must be configured according to the diagram on    *
the OMNI-driver.c file                                     *
* AUTHOR: Team R.A.B.B.I.                                             *
* DATE:		 April 2011  	  																					  *
*																																		  *
* LICENSE: GNU GPL V3 2011                                            *
\**********************************************************************/



/////////////////////////////////////////////////////////////////////////////////////////////////////
//
//                           Tele-Operation Mode Code Template
//
// This file contains a template for simplified creation of an tele-op program for an FTC
// competition.
//
// You need to customize two functions with code unique to your specific robot.
//
/////////////////////////////////////////////////////////////////////////////////////////////////////

#include "drivers/rabbi-common.c"

#ifndef BIG_BOARD
#include "drivers/big_board.h"
#endif


/////////////////////////////////////////////////////////////////////////////////////////////////////
//
//                                    initializeRobot
//
// Prior to the start of tele-op mode, you may want to perform some initialization on your robot
// and the variables within your program.
//
// In most cases, you may not have to add any code to this function and it will remain "empty".
//
/////////////////////////////////////////////////////////////////////////////////////////////////////
#define SPD 80

#define DRIVE_BIAS_FWD 1.02
#define DRIVE_BIAS_RGT 1

#define DRIVE_FACTOR_FWD_A 0.80 * DRIVE_BIAS_FWD
#define DRIVE_FACTOR_FWD_B 0.88 * DRIVE_BIAS_FWD
#define DRIVE_FACTOR_FWD_C 1    * DRIVE_BIAS_FWD
#define DRIVE_FACTOR_FWD_D 0.68 * DRIVE_BIAS_FWD

#define DRIVE_FACTOR_RGT_A 0.80 * DRIVE_BIAS_RGT
#define DRIVE_FACTOR_RGT_B 0.77 * DRIVE_BIAS_RGT
#define DRIVE_FACTOR_RGT_C 1    * DRIVE_BIAS_RGT
#define DRIVE_FACTOR_RGT_D 0.92 * DRIVE_BIAS_RGT

#define DRIVE_FACTOR_BCK_A 0.82
#define DRIVE_FACTOR_BCK_B 0.90
#define DRIVE_FACTOR_BCK_C 1
#define DRIVE_FACTOR_BCK_D 0.70

#define DRIVE_FACTOR_LFT_A 0.82
#define DRIVE_FACTOR_LFT_B 0.77
#define DRIVE_FACTOR_LFT_C 1
#define DRIVE_FACTOR_LFT_D 0.92


void initializeRobot(){
  servo[frontarm] = 0;
  servo[backarm] = 244;
  servo[dispencer] = 5;
  HTSMUXinit();
  HTSMUXscanPorts(SMUX_1);
  HTSMUXscanPorts(SMUX_2);
  wait1Msec(50);
  bDisplayDiagnostics = false;
  kalmindebugdisplay = false;
}

void fwd() {
  hogCPU();
  motor[a] = SPD*0.80;
  motor[b] = SPD*0.85;
  motor[c] = SPD;
  motor[d] = SPD*0.68;
  releaseCPU();
}

void bck() {
  hogCPU();
  motor[a] = -SPD*0.82;
  motor[b] = -SPD*0.90;
  motor[c] = -SPD;
  motor[d] = -SPD*0.70;
  releaseCPU();
}

void rgt() {
  hogCPU();
  motor[a] = -SPD*0.80;
  motor[b] = SPD*0.77;
  motor[c] = -SPD;
  motor[d] = SPD*0.92;
  releaseCPU();
}

void lft() {
  hogCPU();
  motor[a] = SPD*0.82;
  motor[b] = -SPD*0.77;
  motor[c] = SPD;
  motor[d] = -SPD*0.92;
  releaseCPU();
}

void stp() {
  hogCPU();
  motor[d]=0;
  motor[c]=0;
  motor[b]=0;
  motor[a]=0;
  releaseCPU();
}

/*
 * @param degrees the degrees clockwise of "forward" for the direction of movement
 */
void moveDir(int degrees) {
  degrees = 90-degrees;
  degrees %= 360;

  float cosine_d = abs(cosDegrees(degrees));
  float sine_d = abs(sinDegrees(degrees));

  int robotDegrees = degrees - 45;
  float cosine_rob = cosDegrees(robotDegrees);
  float sine_rob = sinDegrees(robotDegrees);

  hogCPU();
  if (degrees>=0 && degrees < 90) { //1st quadrant
	  motor[a]=SPD*sine_rob * ((DRIVE_FACTOR_FWD_A*sine_d) + (DRIVE_FACTOR_RGT_A*cosine_d));
	  motor[b]=SPD*cosine_rob * ((DRIVE_FACTOR_FWD_B*sine_d) + (DRIVE_FACTOR_RGT_B*cosine_d));
	  motor[c]=SPD*sine_rob * ((DRIVE_FACTOR_FWD_C*sine_d) + (DRIVE_FACTOR_RGT_C*cosine_d));
	  motor[d]=SPD*cosine_rob * ((DRIVE_FACTOR_FWD_D*sine_d) + (DRIVE_FACTOR_RGT_D*cosine_d));
  }
  else if (degrees>=90 && degrees <180) { //2nd
	  motor[a]=SPD*sine_rob * ((DRIVE_FACTOR_FWD_A*sine_d) + (DRIVE_FACTOR_LFT_A*cosine_d));
	  motor[b]=SPD*cosine_rob * ((DRIVE_FACTOR_FWD_B*sine_d) + (DRIVE_FACTOR_LFT_B*cosine_d));
	  motor[c]=SPD*sine_rob * ((DRIVE_FACTOR_FWD_C*sine_d) + (DRIVE_FACTOR_LFT_C*cosine_d));
	  motor[d]=SPD*cosine_rob * ((DRIVE_FACTOR_FWD_D*sine_d) + (DRIVE_FACTOR_LFT_D*cosine_d));
  }
  else if (degrees>=180 && degrees <270) { //3rd
	  motor[a]=SPD*sine_rob * ((DRIVE_FACTOR_BCK_A*sine_d) + (DRIVE_FACTOR_LFT_A*cosine_d));
	  motor[b]=SPD*cosine_rob * ((DRIVE_FACTOR_BCK_B*sine_d) + (DRIVE_FACTOR_LFT_B*cosine_d));
	  motor[c]=SPD*sine_rob * ((DRIVE_FACTOR_BCK_C*sine_d) + (DRIVE_FACTOR_LFT_C*cosine_d));
	  motor[d]=SPD*cosine_rob * ((DRIVE_FACTOR_BCK_D*sine_d) + (DRIVE_FACTOR_LFT_D*cosine_d));
  }
  else { //4th
	  motor[a]=SPD*sine_rob * ((DRIVE_FACTOR_BCK_A*sine_d) + (DRIVE_FACTOR_RGT_A*cosine_d));
	  motor[b]=SPD*cosine_rob * ((DRIVE_FACTOR_BCK_B*sine_d) + (DRIVE_FACTOR_RGT_B*cosine_d));
	  motor[c]=SPD*sine_rob * ((DRIVE_FACTOR_BCK_C*sine_d) + (DRIVE_FACTOR_RGT_C*cosine_d));
	  motor[d]=SPD*cosine_rob * ((DRIVE_FACTOR_BCK_D*sine_d) + (DRIVE_FACTOR_RGT_D*cosine_d));
  }
  releaseCPU();

}

task main(){
  initializeRobot();
  int degrees = 0;
  while (true){
    moveDir(degrees);
    degrees+=90;
    wait1Msec(1200);
    stp();
    wait1Msec(400);
    if (degrees>360) { degrees -=360; }
  }
}
